#include <map>
#include <vector>
#include <iostream>

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <errno.h>

#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <linux/serial.h>

#include "serial.hpp"
#include "helper.hpp"

const std::string gsm_fname = "/dev/gsm";
const std::string fp_fname = "/dev/fingerprint";

extern std::map<std::string, Serial> port_map;

static void gsm_init();
static int serial_setup(Serial& device);

fp_packet_t fp_send_request(const fp_packet_t req)
{	
    int fd, rv, read_count;
    fp_packet_t resp = {0x0};

    /* lock the corresponding mutex for I/O. */
    //pthread_mutex_lock(&port_map[fp_fname].mutex);

rewrite:
    fd = port_map[fp_fname].fd;
    rv = write(fd, &req, sizeof(fp_packet_t));

    if(rv == -1) {
        close(fd);
        logger(SM_LOG_ERR,"FINGERPRINT WRITE ERROR: [%s], reconnecting...", strerror(errno));
        open_serial_device(fp_fname);
        goto rewrite;
    }

    if(rv != sizeof(fp_packet_t)){
        logger(SM_LOG_ERR,"FINGERPRINT: unable to send req [%s], port = %s", 
                fp_pkt_to_str(req).c_str(), fp_fname.c_str());
        goto rewrite;
    }

    read_count = 1;

read_again:
    rv = read(fd, &resp, sizeof(resp));

    if(rv <= 0){
        logger(SM_LOG_ERR,"FINGERPRINT TIMEOUT ERROR:");
        goto unlock;
    }

    if(rv != sizeof(fp_packet_t)){
        logger(SM_LOG_ERR,"FINGERPRINT: unable to read resp [%s], port = %s", 
                fp_pkt_to_str(resp).c_str());
        goto unlock;
    }

    logger(SM_LOG_INFO,"FINGERPRINT: [%s]", fp_pkt_to_str(resp).c_str());

    if(req.command == 0x05 && read_count != 0) {
        read_count--;
        goto read_again;
    }

unlock:
    //pthread_mutex_unlock(&port_map[fp_fname].mutex);

    return resp;
}

void fp_send_bcast_msg(const fp_packet_t req)
{	
    int fd, rv;

    /* lock the corresponding mutex for I/O. */
    //pthread_mutex_lock(&port_map[fp_fname].mutex);

rewrite:
    fd = port_map[fp_fname].fd;
    rv = write(fd, &req, sizeof(fp_packet_t));

    if(rv == -1) {
        close(fd);
        logger(SM_LOG_ERR,"FINGERPRINT WRITE ERROR: [%s], reconnecting...", strerror(errno));
        open_serial_device(fp_fname);
        goto rewrite;
    }

    if(rv != sizeof(fp_packet_t)){
        logger(SM_LOG_ERR,"FINGERPRINT WRITE SIZE ERROR: unable to send entire req [%s]", 
                fp_pkt_to_str(req).c_str());
        goto rewrite;
    }

    /* unlock the corresponding mutex for I/O. */
    //pthread_mutex_unlock(&port_map[fp_fname].mutex);
}


/* low level read/write via serial interface for revotech devices */
b_packet_t* rt_send_request(const std::string port, b_packet_t* req)
{	
    char *ptr;
    b_packet_t* resp;
    bool skip_garbage;
    int c, sum, wsize, fd;

    /* lock the corresponding mutex for I/O. */
    pthread_mutex_lock(&port_map[port].mutex);

    sum = 0;
    resp = NULL;
    skip_garbage = true;
    wsize = sizeof(b_packet_t) + req->size;

rewrite:
    fd = port_map[port].fd;
    c = write(fd, req, wsize);
    if(c == -1) {
        logger(SM_LOG_ERR,"WRITE ERROR: port[%s] [%s], reconnecting...", port.c_str(), strerror(errno));
        close(fd);
        open_serial_device(port);
        goto rewrite;
    }

    if(c != wsize){
        logger(SM_LOG_ERR,"WRITE ERROR: unable to send req [%s], port = %s", 
                pkt_to_str(req).c_str(), port.c_str());
        resp = NULL;
        goto unlock;
    }

    resp = (b_packet_t*)malloc(sizeof(b_packet_t));
    if(!resp) {
        logger(SM_LOG_ERR,"PKT ERROR: can't allocate packet for response.");
        goto unlock;
    }

    c = 0;
    ptr = (char*)resp;

    while(true) {
        if(read(fd, ptr, 1) <= 0) {
            logger(SM_LOG_ERR,"READ TIMEOUT ERROR: req[%s], port[%s]", 
                    pkt_to_str(req).c_str(), port.c_str());
            free(resp); resp = NULL;
            goto unlock;
        }

        if(skip_garbage && *ptr != 58) continue;
        else skip_garbage = false;

        if(ptr == (char*)&resp->size) {
            resp = (b_packet_t*)realloc(resp, sizeof(b_packet_t) + resp->size);
            if(!resp) {
                logger(SM_LOG_ERR,"READ ERROR: can't reallocate packet for response.");
                goto unlock;
            }
            ptr = (char*)&resp->size;
            c = resp->size;
        }

        if(ptr >= (char*)&resp->size && c == 0) 
            break;

        ptr++; c--; // TODO: c can be minus.
    }

    /* checksum. */ //TODO
    if(resp->size > 0) {
        ptr = (char*)resp;
        int size = resp->size + sizeof(b_packet_t);
        for(int i = 0; i < size; ++i) {
            sum += *ptr++;
        }
    }

    if(resp->size == 0 || sum % 256 != 0) {
        logger(SM_LOG_ERR,"ERROR [%s]: checksum is not correct [%s]",
                port.c_str(), pkt_to_str(resp).c_str());
        free(resp); resp = NULL;
        goto unlock;
    }

    if(resp->data[0] >= 200 && resp->data[0] <= 250) {
        logger(SM_LOG_ERR,"ERROR reported from [%s]: %s", port.c_str(), pkt_to_str(resp).c_str());
        free(resp); resp = NULL;
        goto unlock;
    }

    if(req->addr != resp->addr) {
        logger(SM_LOG_ERR, "WRONG packet received [%s]: request = [%s], response = [%s]", 
                port.c_str(), pkt_to_str(req).c_str(), pkt_to_str(resp).c_str());
        free(resp); resp = NULL;
        goto unlock;
    }

unlock:
    pthread_mutex_unlock(&port_map[port].mutex);
    return resp;
}

/* write into revotech devices without waiting for response */
void rt_write(const std::string port, b_packet_t* req)
{	
    int c, fd, wsize;
    /* lock the corresponding mutex for I/O. */
    pthread_mutex_lock(&port_map[port].mutex);

    wsize = sizeof(b_packet_t) + req->size;

rewrite:
    fd = port_map[port].fd;
    c = write(fd, req, wsize);
    if(c == -1) {
        logger(SM_LOG_ERR,"WRITE ERROR: port[%s] [%s], reconnecting...", port.c_str(), strerror(errno));
        close(fd);
        open_serial_device(port);
        goto rewrite;
    }

    if(c != wsize)
        logger(SM_LOG_ERR,"WRITE ERROR: unable to send req [%s], port = %s", 
                pkt_to_str(req).c_str(), port.c_str());

    pthread_mutex_unlock(&port_map[port].mutex);
    return;
}

/* GSM */
int gsm_write(char *chars) 
{
    int len = strlen(chars), fd;
    // stick a after the command        (0xd == 13 == ASCII CR)
    chars[len] = 0x0d;

    /* lock the corresponding mutex for I/O. */
    //pthread_mutex_lock(&port_map[gsm_fname].mutex);

rewrite:
    fd = port_map[gsm_fname].fd;
    // terminate the string properly
    chars[len+1] = 0x00;
    int n = write(fd, chars, strlen(chars));
    if(n == -1) {
        close(fd);
        logger(SM_LOG_ERR,"GSM WRITE ERROR: [%s], reconnecting...", strerror(errno));
        open_serial_device(gsm_fname);
        goto rewrite;
    }

    //pthread_mutex_unlock(&port_map[gsm_fname].mutex);
    return 0;
}

int gsm_read(char *result) 
{
    int fd;
    /* lock the corresponding mutex for I/O. */
    //pthread_mutex_lock(&port_map[gsm_fname].mutex);

    fd = port_map[gsm_fname].fd;
    int n = read(fd, result, 254);
    if(n == -1) {
        close(fd);
        logger(SM_LOG_ERR,"GSM READ ERROR: [%s], reconnecting...", strerror(errno));
        open_serial_device(gsm_fname);
        goto unlock;
    }

    result[n-1] = 0x00; 
unlock:
    //pthread_mutex_unlock(&port_map[gsm_fname].mutex);
    return 0;
}

static int serial_setup(Serial& device)
{
    struct termios stbuf;

    memset (&stbuf, 0, sizeof (stbuf)); 
    if (tcgetattr (device.fd, &stbuf) != 0) {
        perror("tcgetattr ");
        logger(SM_LOG_ERR, "FAILED TO GET SERIAL ATTR [%d]", device.fd);
        return -1;
    }

    stbuf.c_cflag &= ~(CBAUD | CSIZE | CSTOPB | CLOCAL | PARENB);
    stbuf.c_iflag &= ~(HUPCL | IUTF8 | IUCLC | ISTRIP | IXON | IXOFF | IXANY | ICRNL);
    stbuf.c_oflag &= ~(OPOST | OCRNL | ONLCR | OLCUC | ONLRET);
    stbuf.c_lflag &= ~(ICANON | ISIG | IEXTEN | ECHO | ECHOE | ECHOK | ECHONL);
    stbuf.c_lflag &= ~(NOFLSH | XCASE | TOSTOP | ECHOPRT | ECHOCTL | ECHOKE);
    stbuf.c_cc[VMIN] = 0;
    stbuf.c_cc[VTIME] = device.timeout;
    stbuf.c_cc[VEOF] = 1;
    stbuf.c_cflag |= (device.speed | CREAD | 0 | 0);  /* No parity, 1 stop bit */
    errno = 0;
    if (tcsetattr (device.fd, TCSANOW, &stbuf) < 0) {
        perror("tcgetattr");
        logger(SM_LOG_ERR, "FAILED TO SET SERIAL ATTR [%d]", device.fd);
        return -1;
    }
    return 0;
}

static void gsm_init()
{
    for(int i = 0; i < 4; ++i) {
        char tmp[4];
        char buff[64];
        tmp[0] = 'A';
        tmp[1] = 'T';
        tmp[2] = 0;
        logger(SM_LOG_INFO,"GSM INIT...");
        gsm_write(tmp);
        gsm_read(buff);
        logger(SM_LOG_INFO,"GSM INIT DONE");
    }
}

void open_serial_device(std::string port)
{
    int fd;
    struct termios termio;

    Serial& device = port_map[port];

    memset (&termio, 0, sizeof (termio));

    while(true) {
reopen:
        fd = open(port.c_str(), O_RDWR | O_NDELAY | O_EXCL | O_NONBLOCK | O_NOCTTY);
        if(fd != -1) {
            device.fd = fd;
            break;
        }
        sleep(1);
        logger(SM_LOG_ERR,"Unable to open the port %s", port.c_str());
    }

    if (ioctl (fd, TIOCEXCL) < 0) {
        logger (SM_LOG_ERR, "Could not lock serial device[%s]: err[%s]",
                port.c_str(), strerror (errno));
        close(fd);
        goto reopen;
    }

    /* Flush any waiting IO */
    tcflush (fd, TCIOFLUSH);

    if(serial_setup(device) < 0) {
        close(fd);
        goto reopen;
    }

    fcntl(fd, F_SETFL, 0);
    logger(SM_LOG_INFO, CGRN"Successfuly connected PORT[%s] SPEED[%d] "
            "TIMEOUT[%d]"CNRM, port.c_str(), device.speed, device.timeout);

    if(port == gsm_fname) {
        gsm_init();
    }

    if(port == fp_fname) {
        fp_init();
    }
}

